#ifndef __POSESOLVER_
#define __POSESOLVER_

#include <open3d/Open3D.h>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include <memory.h>
#include <string.h>

using namespace open3d;

class PoseSolver;
using p_posesolver = std::shared_ptr<PoseSolver>;

cv::Mat ImageToMat(const geometry::Image &image);
geometry::Image MatToImage(const cv::Mat &mat);

class PoseSolver : visualization::Visualizer
{
public:
    // 设置默认构造函数
    using visualization::Visualizer::Visualizer;
    ~PoseSolver(){};
    // 姿态解算器执行解决方案
    void resolute(const cv::Mat &, const cv::Mat &, const rs2_intrinsics &, cv::Rect);
    // 点云可视化
    void pcdShow();

private:
    // 姿态解算器初始化
    void _solverInit(const cv::Mat &, const cv::Mat &, const rs2_intrinsics &);
    // 准备数据
    void _prepare(const cv::Mat &, const cv::Mat &, cv::Rect);
    // 矩形框拓展
    cv::Rect _rectExpand(const cv::Rect &);
    // 加载 librealsense 中的相机内参并转成 PinholeCameraIntrinsic
    camera::PinholeCameraIntrinsic _loadIntrinsics(const rs2_intrinsics &);

private:
    // rgb image格式影像流数据
    geometry::Image _frame;
    // depth image格式影像流数据
    geometry::Image _depth;
    // roi区域 rgb image格式影像流数据
    geometry::Image _depRoiImage;
    // roi区域 depth image格式影像流数据
    geometry::Image _rgbRoiImage;
    // roi区域 depth mat格式影像流数据
    cv::Mat _DepRoiMat;
    // roi区域 rgb mat格式影像流数据
    cv::Mat _RgbRoiMat;
    // 相机内参
    camera::PinholeCameraIntrinsic _intrinsics;
    // rgbd图像
    std::shared_ptr<geometry::RGBDImage> _rgbd;
    // 点云图像
    std::shared_ptr<geometry::PointCloud> _pcd;
};

#endif
